/*
 * @Description: ICP 匹配模块
 * @Author: Ren Qian
 * @Date: 2020-02-08 21:46:45
 */
#include "lidar_localization/models/registration/icp_registration.hpp"

#include "glog/logging.h"

namespace lidar_localization {

ICPRegistration::ICPRegistration(const YAML::Node& node)
    :icp_ptr_(new pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>()) {
    
    //获取参数
    float max_correspondence_distance = node["max_correspondence_distance"].as<float>();
    float max_iter = node["max_iter"].as<int>();
    float trans_eps = node["trans_eps"].as<float>();
    int euclidean_fitness_eps = node["euclidean_fitness_eps"].as<float>();
    int ransac_iter = node["ransac_iter"].as<int>();

    SetRegistrationParam(max_correspondence_distance, max_iter, trans_eps,euclidean_fitness_eps,ransac_iter);
}

ICPRegistration::ICPRegistration(float max_correspondence_distance,int  max_iter, float trans_eps, float euclidean_fitness_eps,int ransac_iter)
    :icp_ptr_(new pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>()) {

    SetRegistrationParam(max_correspondence_distance, max_iter, trans_eps,euclidean_fitness_eps,ransac_iter);
}

bool ICPRegistration:: SetRegistrationParam(float max_correspondence_distance,int  max_iter, float trans_eps, float euclidean_fitness_eps,int ransac_iter) {
    //设置参数
    icp_ptr_->setMaxCorrespondenceDistance(max_correspondence_distance);
    icp_ptr_->setMaximumIterations(max_iter);
    icp_ptr_->setTransformationEpsilon(trans_eps);
    icp_ptr_->setEuclideanFitnessEpsilon(euclidean_fitness_eps);
    icp_ptr_->setRANSACIterations(ransac_iter);

 LOG(INFO) << "ICP 的匹配参数为：" << std::endl
              << "max_correspondence_distance: " << max_correspondence_distance << ", "
              << "max_iter: " << max_iter << ", "
              << "trans_eps: " << trans_eps << ", "
              << "euclidean_fitness_eps: " << euclidean_fitness_eps << ","
              << "ransac_iter: " << ransac_iter << ","
              << std::endl << std::endl;
    return true;
}

bool ICPRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) {
    icp_ptr_->setInputTarget(input_target);

    return true;
}

bool ICPRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source, 
                                const Eigen::Matrix4f& predict_pose, 
                                CloudData::CLOUD_PTR& result_cloud_ptr,
                                Eigen::Matrix4f& result_pose) {
    icp_ptr_->setInputSource(input_source);
    icp_ptr_->align(*result_cloud_ptr, predict_pose);
    result_pose = icp_ptr_->getFinalTransformation();

    return true;
}
}
